package com.googlecode.gunncs.mechanism;

import java.util.ArrayList;

import com.googlecode.grtframework.core.GRTLoggedProcess;
import com.googlecode.grtframework.event.EncoderEvent;
import com.googlecode.grtframework.event.EncoderListener;
import com.googlecode.grtframework.sensor.IEncoder;

/**
 * A SimWheelEncoder is commanded as a drive train and publishes Encoder Events
 * that represent such motion
 * 
 * It is a way to seamlessly simulate and visualize motion.
 * 
 * @author ajc
 * 
 */
public class SimWheelEncoder extends GRTLoggedProcess implements IWheel,
		IEncoder {

	private static final int TIMESTEP = 1;

	private ArrayList<EncoderListener> listeners;

	private double prevCount;
	private double count;
	private double velocity;
	private boolean direction;

	/**
	 * Units... to be figured out
	 */
	public SimWheelEncoder() {
		super("encoder");
		listeners = new ArrayList<EncoderListener>();
	}

	public void run() {
		running = true;
		while (running) {

			if (!suspended) {
				prevCount = count;
				// integrate velocity into count
				count += velocity * TIMESTEP;

				// notify count change
				if (velocity != 0) {
					notifyCountChanged();
				}
				// notify rotation changes (start, stop direction change
				// TODO

			}
			try {
				Thread.sleep(TIMESTEP);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
	}

	@Override
	public void setVelocity(double velocity) {
		this.velocity = velocity;
	}

	@Override
	public void addEncoderListener(EncoderListener l) {
		listeners.add(l);
	}

	@Override
	public void removeEncoderListener(EncoderListener l) {
		listeners.remove(l);
	}

	private void notifyCountChanged() {
		EncoderEvent ev = new EncoderEvent(this, EncoderEvent.DISTANCE, count
				- prevCount, velocity > 0);
		// count-prevCount: change of position
		// velocity>0: indicates direction
		for (EncoderListener l : listeners) {
			l.countDidChange(ev);
		}
	}

	private void notifyRotationStarted() {
		// System.out.println("rotation start");
	}

	private void notifyRotationStopped() {
		// System.out.println("rotation stop");
	}

	private void notifyDirectionChanged() {
		// System.out.println("direction changed");
	}

}
